/**
 * @file CliffFollowPlanner.h
 * @author 
 * @brief 沿悬崖规划(6个传感器) 通过探测悬崖点，计算出一条直线，控制机器人与直线的距离,在计算出一条直线之前，传感器触发进行后退转向。计算出直线后，转向与直线平行角度，并慢慢趋向悬崖，再次触发悬崖后重新调整方向
 * @version 1.0
 * @date 2020-08-18
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include <cstdint>
#include <vector>
#include "data_struct/robot/CliffData.h"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace behavior_controller
{

    class CliffFollowPlanner
    {
    public:
        /**
         * @brief 沿悬崖规划的构造函数
         * @return
         */
        CliffFollowPlanner(){};

        /**
         * @brief 沿悬崖规划的运行函数
         * @return
         */
        void run(double &linear_vel, double &angular_vel, bool & around_finished);

        /**
         * @brief 沿悬崖边界时的PID调节函数
         * @return
         */
        void pid_adjustment(double &robot_dis, double &linear_vel, double &angular_vel);

        /**
         * @brief 控制机器人后退转向函数
         * @return
         */
        void BackAndTurn(double &linear_vel, double &angular_vel);

        /**
         * @brief 控制机器人转向与悬崖边界平行方向的函数
         * @return
         */
        void TurnToAngle(double &linear_vel, double &angular_vel);
        void SetFinishTrue();
        void SetFinishTrueClearPose();
        void SetStartPose();

        double last_dis_p;     //记录上一次的距离偏差，用于计算微分环节的偏差之差
        double angle_diff;     //检测到悬崖直线后，直线的
        double angle_line_global;
        bool finished = true; //沿悬崖动作是否结束的标记
        bool turn_finished=false;
        std::vector<PoseF> point_set; //用于记录悬崖边界点的点集
        geometry_msgs::msg::PoseStamped start_pose;
        PoseF start_point, end_point; //所找到的悬崖线上的点
        double time_now = 0;          //记录机器人进入沿悬崖状态时的时间，沿悬崖行走超过5s后，退出沿悬崖，进入沿障碍物
        double time_now2 = 0;           //dpj
        double time_now3 = 0;           //dpj
    private:
        uint8_t sensor_bit_;    //沿边传感器按位表示，0x000000依次顺序为左、左中、左前、右前、右中、右
        double dis_;             //机器人与计算出直线的距离
        double line_k, line_b,R;
        bool line_flag = false; //是否找到悬崖边界线的标记
    };

    //static CliffFollowPlanner cliff_follow_planner; //实例化沿悬崖类
    extern CliffFollowPlanner cliff_follow_planner; //实例化沿悬崖类
} // namespace behavior_controller